#include "../include/battery.h"

#include <string>
using namespace std;

Battery::Battery(ros::NodeHandle nh, ros::NodeHandle n) :
    node_handle_(nh),
	node_h_(n),
    _port("/dev/battery"),
    _frequency(2),
    _version(1)
{
  node_h_.param("port", _port, _port);
  node_h_.param("frequency",_frequency,_frequency);
  node_h_.param("version",_version,_version);
    //通信命令
  if(_version == 1)
  {
    _frequency = _frequency * 2;
    analog_quantity = "~25014642E00201FD30\r";
    warning_lamp = "~25014644E00201FD2E\r";
  }
  else if(_version == 2)
  {
    analog_quantity = ":000200000E08~";
  }

	batSerial = new serial::Serial;
    try
    {
        batSerial->setPort(_port);
        batSerial->setBaudrate(9600);
        serial::Timeout to = serial::Timeout::simpleTimeout(1000);
        batSerial->setTimeout(to);
        batSerial->open();
    }
    catch(serial::IOException& e)
    {
        ROS_ERROR_STREAM("Unable to open port !");
        return;
    }
    if(batSerial->isOpen())
    {
        ROS_INFO_STREAM("Serial Port initialized,The battery is powered normally!");
    }
    else
    {
        return;
    }
    //ROS
    battery_pub = node_handle_.advertise<sensor_msgs::BatteryState>("battery",10);
}

Battery::~Battery()
{
    batSerial->close();
}

bool Battery::spin()
{
	ROS_INFO("_frequency:%d",_frequency);
	ROS_INFO("_version:%d",_version);
	ROS_INFO_STREAM(analog_quantity);
    ros::Rate r(_frequency);
    while (ros::ok())
    {
        if(_version == 3)
        {
            uint8_t quantity[6] = {0x7f, 0x10, 0x02, 0x06, 0x10, 0x59};
            batSerial->write(quantity, 6);
        }
        else
        {
            batSerial->write(analog_quantity);
        }
        
		
		ros::spinOnce();
		r.sleep();
		while (batSerial->available())
		{
            if(_version == 3)
            {
                uint8_t _data_temp[40];
                batSerial->read(_data_temp, 40);
                analogParseJG(_data_temp);
            }
            else
            {
                string soi = batSerial->read();
                if(soi == ":")
                {
                    int count = batSerial->available();
                    string analog_data;
                    batSerial->read(analog_data,count);
                    analogParse(analog_data);
                }
                else if(soi == "~")
                {
                    int count = batSerial->available();
                    ROS_INFO("count:%d",count);
                    string analog_data;
                    batSerial->read(analog_data,count);
                    //ROS_INFO_STREAM(analog_data);
                    analogParse_64V(analog_data);
                }
            }
		}
		if(_version == 1)
		{
			batSerial->write(warning_lamp);
			r.sleep();
			while (batSerial->available())
			{
				string soi = batSerial->read();
				if(soi == "~")
				{
					int count = batSerial->available();
					string analog_data;
					batSerial->read(analog_data,count);
					analogParse_64V_warning(analog_data);
				}
			}
		}
		 
    }
}

void Battery::analogParse(string _data)
{
    sensor_msgs::BatteryState battery_analog;
    char *offset;

    battery_analog.location = ":" + _data;

    battery_analog.header.stamp = ros::Time::now();;
    battery_analog.header.frame_id = "battery";
    battery_analog.present = true;

    //Vbat 电池电压
    string _data_s;
    _data_s.assign(_data,24,4);
    int _voltage_i = strtol(_data_s.c_str(),&offset,16);
    battery_analog.voltage = (float)_voltage_i * 2 / 1000;

    //每一节电压 mV
    _data_s.assign(_data,28,2);
    int _cell_num_i = strtol(_data_s.c_str(),&offset,16);
    battery_analog.cell_voltage.resize(_cell_num_i);
    for(int i=0;i<_cell_num_i;i++)
    {
        _data_s.assign(_data,30+i*4,4);
        int _cell_voltage_i = strtol(_data_s.c_str(),&offset,16);
        battery_analog.cell_voltage[i] = (float)_cell_voltage_i / 1000;
    }

    //充电电流
    _data_s.assign(_data,62,4);
    int _curr_0_i = strtol(_data_s.c_str(),&offset,16);
    battery_analog.charge = (float)_curr_0_i / 100;
    //放电电流
    _data_s.assign(_data,66,4);
    int _curr_1_i = strtol(_data_s.c_str(),&offset,16);
    battery_analog.current = (float)_curr_1_i / 100;

    //电池百分比
    _data_s.assign(_data,128,2);
    int _soc_i = strtol(_data_s.c_str(),&offset,16);
    battery_analog.percentage = (float)_soc_i / 100;

    //当前容量
    _data_s.assign(_data,130,4);
    int _cap_now_i = strtol(_data_s.c_str(),&offset,16);
    battery_analog.capacity = (float)_cap_now_i / 10;

    //设计容量
    _data_s.assign(_data,134,4);
    int _cap_full_i = strtol(_data_s.c_str(),&offset,16);
    battery_analog.design_capacity = (float)_cap_full_i / 10;

    battery_pub.publish(battery_analog);
}

void Battery::analogParse_64V(string _data)
{
	sensor_msgs::BatteryState battery_analog_64v;
	char *offset;

    battery_analog_64v.location = "~" + _data;

    battery_analog_64v.header.stamp = ros::Time::now();;
    battery_analog_64v.header.frame_id = "battery";
    battery_analog_64v.present = true;
	
	//Vbat 电池电压
	string _data_s;
	_data_s.assign(_data,16,2);
    int _cell_num_i = strtol(_data_s.c_str(),&offset,16);
	battery_analog_64v.cell_voltage.resize(_cell_num_i);
    for(int i=0;i<_cell_num_i;i++)
    {
        _data_s.assign(_data,18+i*4,4);
        int _cell_voltage_i = strtol(_data_s.c_str(),&offset,16);
        battery_analog_64v.cell_voltage[i] = (float)_cell_voltage_i / 1000;
    }
	
    _data_s.assign(_data,112,4);
    int _voltage_i = strtol(_data_s.c_str(),&offset,16);
    battery_analog_64v.voltage = (float)_voltage_i / 1000;
	
	//充电电流
    _data_s.assign(_data,108,4);
    int _curr_0_i = strtol(_data_s.c_str(),&offset,16);
    battery_analog_64v.charge = (float)_curr_0_i / 100;
    //放电电流
    _data_s.assign(_data,108,4);
    int _curr_1_i = strtol(_data_s.c_str(),&offset,16);
    battery_analog_64v.current = (float)_curr_1_i / 100;

    //当前容量
    _data_s.assign(_data,116,4);
    int _cap_now_i = strtol(_data_s.c_str(),&offset,16);
    battery_analog_64v.capacity = (float)_cap_now_i / 100;

    //设计容量
    _data_s.assign(_data,122,4);
    int _cap_full_i = strtol(_data_s.c_str(),&offset,16);
    battery_analog_64v.design_capacity = (float)_cap_full_i / 100;
	
	//电池百分比
	float pec = (float)_cap_now_i/(float)_cap_full_i;
    battery_analog_64v.percentage = pec;
	
	battery_pub.publish(battery_analog_64v);
}

void Battery::analogParse_64V_warning(string _data)
{
	ROS_INFO_STREAM(_data);
}

void Battery::analogParseJG(uint8_t *data_)
{
    sensor_msgs::BatteryState battery_analog_JG;

    battery_analog_JG.header.stamp = ros::Time::now();;
    battery_analog_JG.header.frame_id = "battery";
    battery_analog_JG.present = true;
	
	//Vbat 电池电压
    int _cell_num_i = data_[11];
	battery_analog_JG.cell_voltage.resize(_cell_num_i);
    float all_voltage = 0.0;
    for(int i=0;i<_cell_num_i;i++)
    {
        int _cell_voltage_i = data_[13+i*2]*256+data_[12+i*2];
        battery_analog_JG.cell_voltage[i] = (float)_cell_voltage_i / 1000;
        all_voltage += battery_analog_JG.cell_voltage[i];
    }
	
    battery_analog_JG.voltage = all_voltage;
	
	//充电放电电流
	uint8_t flag = data_[10] & 0x80;
	if(flag==0x80)
	{
	battery_analog_JG.current=(double)(~data_[9])/100; //放电电流
	}
	else
	{
	battery_analog_JG.charge=(double)data_[9]/10;//充电电流
	}
    //当前容量
    int _cap_now_i = data_[35]*256+data_[34];
    battery_analog_JG.capacity = (float)_cap_now_i / 10;

    //设计容量
    int _cap_full_i = data_[37]*256+data_[36];
    battery_analog_JG.design_capacity = (float)_cap_full_i / 10;
	
	//电池百分比
	float pec = (float)_cap_now_i/(float)_cap_full_i;
    battery_analog_JG.percentage = pec;
	
	battery_pub.publish(battery_analog_JG);
}